{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "#%pylab inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "import cv2, Leap, math, ctypes\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "import Leap"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "\n",
    "controller = Leap.Controller()\n",
    "controller.set_policy_flags(Leap.Controller.POLICY_IMAGES)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "frame = controller.frame()\n",
    "image = frame.images[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "False"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "image.is_valid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "if image.is_valid:\n",
    "    if not maps_initialized:\n",
    "        left_coordinates, left_coefficients = convert_distortion_maps(frame.images[0])\n",
    "        right_coordinates, right_coefficients = convert_distortion_maps(frame.images[1])\n",
    "        maps_initialized = True\n",
    "\n",
    "    undistorted_left = undistort(image, left_coordinates, left_coefficients, 400, 400)\n",
    "    undistorted_right = undistort(image, right_coordinates, right_coefficients, 400, 400)\n",
    "\n",
    "    #display images\n",
    "    cv2.imshow('Left Camera', undistorted_left)\n",
    "    cv2.imshow('Right Camera', undistorted_right)\n",
    "\n",
    "    if cv2.waitKey(1) & 0xFF == ord('q'):\n",
    "        break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "import cv2, Leap, math, ctypes\n",
    "import numpy as np\n",
    "\n",
    "def convert_distortion_maps(image):\n",
    "\n",
    "    distortion_length = image.distortion_width * image.distortion_height\n",
    "    xmap = np.zeros(distortion_length/2, dtype=np.float32)\n",
    "    ymap = np.zeros(distortion_length/2, dtype=np.float32)\n",
    "\n",
    "    for i in range(0, distortion_length, 2):\n",
    "        xmap[distortion_length/2 - i/2 - 1] = image.distortion[i] * image.width\n",
    "        ymap[distortion_length/2 - i/2 - 1] = image.distortion[i + 1] * image.height\n",
    "\n",
    "    xmap = np.reshape(xmap, (image.distortion_height, image.distortion_width/2))\n",
    "    ymap = np.reshape(ymap, (image.distortion_height, image.distortion_width/2))\n",
    "\n",
    "    #resize the distortion map to equal desired destination image size\n",
    "    resized_xmap = cv2.resize(xmap,\n",
    "                              (image.width, image.height),\n",
    "                              0, 0,\n",
    "                              cv2.INTER_LINEAR)\n",
    "    resized_ymap = cv2.resize(ymap,\n",
    "                              (image.width, image.height),\n",
    "                              0, 0,\n",
    "                              cv2.INTER_LINEAR)\n",
    "\n",
    "    #Use faster fixed point maps\n",
    "    coordinate_map, interpolation_coefficients = cv2.convertMaps(resized_xmap,\n",
    "                                                                 resized_ymap,\n",
    "                                                                 cv2.CV_32FC1,\n",
    "                                                                 nninterpolation = False)\n",
    "\n",
    "    return coordinate_map, interpolation_coefficients"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "def undistort(image, coordinate_map, coefficient_map, width, height):\n",
    "    destination = np.empty((width, height), dtype = np.ubyte)\n",
    "\n",
    "    #wrap image data in numpy array\n",
    "    i_address = int(image.data_pointer)\n",
    "    ctype_array_def = ctypes.c_ubyte * image.height * image.width\n",
    "    # as ctypes array\n",
    "    as_ctype_array = ctype_array_def.from_address(i_address)\n",
    "    # as numpy array\n",
    "    as_numpy_array = np.ctypeslib.as_array(as_ctype_array)\n",
    "    img = np.reshape(as_numpy_array, (image.height, image.width))\n",
    "\n",
    "    #remap image to destination\n",
    "    destination = cv2.remap(img,\n",
    "                            coordinate_map,\n",
    "                            coefficient_map,\n",
    "                            interpolation = cv2.INTER_LINEAR)\n",
    "\n",
    "    #resize output to desired destination size\n",
    "    destination = cv2.resize(destination,\n",
    "                             (width, height),\n",
    "                             0, 0,\n",
    "                             cv2.INTER_LINEAR)\n",
    "    return destination"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "depth-camera",
   "language": "python",
   "name": "depth-camera"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.16"
  },
  "latex_envs": {
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 0
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
